#include "hash.h"
#include "prime.h"
#include <stdio.h>

void print_particle(Particle *p) {
    printf("%d:\n\tpos: <%f, %f, %f>"
     "\n\tpressure: <%f, %f, %f>\n\tviscosity: <%f, %f, %f>"
     "\n\tgravity <%f, %f, %f>\n\tsurface tension: <%f, %f, %f>\n\t"
     "accel: <%f, %f, %f>\n\tvelocity: <%f, %f, %f>\n\t"
     "v_half: <%f, %f, %f>\n\t"
     "normal: <%f, %f, %f>\n\t"
     "pressure (particle): %f\n\t"
     "density: %f\n\t"
     "mass: %f\n",
     p->id, 
     p->pos.x, p->pos.y, p->pos.z,
     p->f_pressure.x,p->f_pressure.y,p->f_pressure.z, 
     p->f_viscosity.x,p->f_viscosity.y,p->f_viscosity.z, 
     p->f_gravity.x,p->f_gravity.y,p->f_gravity.z,
     p->f_surface.x,p->f_surface.y,p->f_surface.z,
     p->a.x, p->a.y, p->a.z,
     p->v.x, p->v.y, p->v.z,
     p->v_half.x, p->v_half.y, p->v_half.z,
     p->normal.x, p->normal.y, p->normal.z,
     p->pressure, p->density, p->mass);
}

void print_list(Node *list) {
    Node *temp;
    int count = 1;

    if (!list) {
        printf("Empty list\n");
        return;
    }

    temp = list;
    while (temp) {
        if (!temp->p)
            continue;

        printf("Particle %d: %d\n", count, temp->p->id);
        print_particle(temp->p);
        temp = temp->next;
        count++;
    }

    printf("\tFinsihed printing\n");
}

int HashTable::hash(glm::vec3 v) {
    int rx, ry, rz;

    rx = (int) (v.x / dist);
    ry = (int) (v.y / dist);
    rz = (int) (v.z / dist);


    rx = (rx * PRIME_1 ^ ry * PRIME_2 ^ rz * PRIME_3) % size;
    while (rx < 0) 
        rx += size;

    return rx;
}

bool notInList(Node *list, Particle *p) {
    Node *temp;

    temp = list;
    while (temp) {
        if (temp->p->id == p->id)
            return false;

        temp = temp->next;
    }

    return true;
}

Node *merge(Node *n1, Node *n2) {
    Node *ret, *temp, *walk;
    int skip = 0;

    walk = n2;
    ret = n1;
    if (!walk)
        return ret;

    if (!ret) {
        ret = new Node();

        ret->p = walk->p;
        walk = walk->next;
    }

    temp = ret;

    /* find the end of the list */
    while (temp->next)
        temp = temp->next;

    /* append to the end of the list */
    while (walk) {
        if (notInList(ret, walk->p)) {
            temp->next = new Node();
            temp->next->p = walk->p;
            temp = temp->next;
        }

        walk = walk->next;
    }

    return ret;
}

Node *HashTable::cull(Node *list, glm::vec3 c, int id) {
    Node *walk1, *ret, *ret_walk;

    walk1 = list;

    while (walk1) {
        if (walk1->p && glm::length(walk1->p->pos - c) < dist && 
         walk1->p->id != id)
            break;
        walk1 = walk1->next;
    }

    if (!walk1)
        return NULL;

    ret = new Node();
    ret->p = walk1->p;
    walk1 = walk1->next;

    while (walk1) {
        if (glm::length(walk1->p->pos - c) <= dist && walk1->p->id != id) {
            ret_walk = new Node();
            ret_walk->p = walk1->p;
            ret_walk->next = ret->next;
            ret->next = ret_walk;
        }

        walk1 = walk1->next;
    }

    delete list;

    return ret;
}


Node *HashTable::lookUp(float x, float y, float z) {
    float xmin, ymin, zmin, xmax, ymax, zmax;
    Node *temp, *ret;
    int ix, iy, iz;

    xmin = (x - dist) / dist;
    ymin = (y - dist) / dist;
    zmin = (z - dist) / dist;
    xmax = (x + dist) / dist;
    ymax = (y + dist) / dist;
    zmax = (z + dist) / dist;

    temp = NULL;

    for (ix = -1; ix < 2; ix++) {
        for (iy = -1; iy < 2; iy++) {
            for (iz = -1; iz < 2; iz++) {
                printf("ix: %d, iy: %d, iz: %d, hash: %d\n", ix, iy, iz,
                    hash(glm::vec3(x + dist * ix, y + dist * iy, z + dist * iz)));
                temp = merge(temp, nodes[hash(glm::vec3(
                 x + dist * ix,
                 y + dist * iy,
                 z + dist * iz))].next);
            }
        }
    }

    ret = cull(temp, glm::vec3(x, y, z), -1);
    //print_list(ret);

    return ret;
}

Node *HashTable::lookUpId(float x, float y, float z, int id) {
    float xmin, ymin, zmin, xmax, ymax, zmax;
    Node *temp, *ret;
    int ix, iy, iz;

    xmin = (x - dist) / dist;
    ymin = (y - dist) / dist;
    zmin = (z - dist) / dist;
    xmax = (x + dist) / dist;
    ymax = (y + dist) / dist;
    zmax = (z + dist) / dist;

    temp = NULL;

    for (ix = -1; ix < 2; ix++) {
        for (iy = -1; iy < 2; iy++) {
            for (iz = -1; iz < 2; iz++) {
                temp = merge(temp, nodes[hash(glm::vec3(
                 x + dist * ix,
                 y + dist * iy,
                 z + dist * iz))].next);
            }
        }
    }

    ret = cull(temp, glm::vec3(x, y, z), id);
    //if (id == 484)
    //print_list(ret);

    return ret;
}

void HashTable::insert(Particle *p) {
    Node *temp, *temp_walk;

    temp = &nodes[hash(p->pos)];

    if (temp->next)
        temp_walk = temp->next;
    else {
        temp->next = new Node();
        temp->next->next = NULL;
        temp->next->p = p;
        return;
    }

    while (temp_walk && temp_walk->next) {
        temp_walk = temp_walk->next;
    }

    if (!temp_walk) {
        temp->next = new Node();
        temp->next->next = NULL;
        temp->next->p = p;
        return;
    }

    temp_walk->next = new Node();
    temp_walk->next->p = p;
}

void HashTable::remove(Particle *p) {
    Node *head, *temp, *temp_walk;

    head = &nodes[hash(p->pos_prev)];
    temp = head->next;

    if (!temp) 
        return;
    else if (!temp->next && temp->p->id == p->id) {
        head->next = NULL;
        delete temp;
        return;
    }
    else if (!temp->next)
        return;

    temp_walk = temp->next;
    while (temp_walk) {
        if (temp_walk->p->id == p->id) {
            temp->next = temp_walk->next;
            temp_walk->next = NULL;
            delete temp_walk;
            return;
        }

        temp = temp_walk;
        temp_walk = temp_walk->next;
    }
}
